#include <QDebug>
#include <QList>
#include "astarsearch.h"

AStarSearchNode::AStarSearchNode(QObject *parent):
    QObject(parent) {
    m_g = 0;
    m_h = 0;
    m_f = 0;
    m_came_from = NULL;
}

void AStarSearchNode::setG(float g) {
    m_g = g;
}
void AStarSearchNode::setH(float h) {
    m_h = h;
}
void AStarSearchNode::setF(float f) {
    m_f = f;
}
const AStarSearchNode* AStarSearchNode::cameFrom() const {
    return m_came_from;
}
void AStarSearchNode::setCameFrom(const AStarSearchNode* cameFrom) {
    m_came_from = cameFrom;
}
float AStarSearchNode::g() const {
    return m_g;
}
float AStarSearchNode::h() const {
    return m_h;
}
float AStarSearchNode::f() const {
    return m_f;
}
bool AStarSearchNode::operator<(const AStarSearchNode& other) const {
    return m_f < other.f();
}

bool AStarSearchNode::lessThan(const AStarSearchNode* s1, const AStarSearchNode* s2) {
    return s1->m_f < s2->m_f;
}

AStarSearch::AStarSearch(QObject *parent) :
    QObject(parent)
{
    m_searching = false;
}

AStarSearch::~AStarSearch() {
    freeNodes();
}

void AStarSearch::freeNodes() {
    while (m_nodes.count()) {
        AStarSearchNode * node = m_nodes.front();
        delete node;
        m_nodes.pop_front();
    }
}

bool AStarSearch::seaching() {
    return m_searching;
}

bool AStarSearch::search(AStarSearchNode& start,
                         AStarSearchNode& goal,
                         QList<AStarSearchNode*>& pathToGoal) {
    if (m_searching) {
        qDebug()<<"Already searching, do nothing";
        return false;
    }
    qDebug()<<"search start"<<start.nodeInfo()<<"goal"<<goal.nodeInfo();
    m_searching = true;
    freeNodes();
    //closedset := the empty set
    QList<AStarSearchNode*> closedSet;
    //openset := set containing the initial node
    QList<AStarSearchNode*> openSet;
    openSet.append(&start);

    //g_score[start] := 0
    start.setG(0);
    //h_score[start] := heuristic_estimate_of_distance(start, goal)
    start.setH(start.heuristicEstimateOfDistance(goal));
    //f_score[start] := h_score[start]
    start.setF(start.h());

    //while openset is not empty
    while (openSet.count()) {
        //x := the node in openset having the lowest f_score[] value
        qSort(openSet.begin(), openSet.end(), AStarSearchNode::lessThan);
        AStarSearchNode* x = openSet.front();
        qDebug()<<"Get first node from openSet"<<x->nodeInfo();

        //if x = goal
        if (*x == goal) {
            //return reconstruct_path(came_from,goal);
            qDebug()<<"Reach the goal, generateing path"<<goal.nodeInfo();
            pathTo(*x, start, pathToGoal);
            m_searching = false;
            return true;
        }

        //remove x from openset
        x = openSet.first();
        openSet.removeFirst();
        //add x to closedset
        closedSet.append(x);
        qDebug()<<"Add node to closedSet"<<x->nodeInfo();

        //Get x's neighbors
        QList<AStarSearchNode*> neighbors;
        x->neighborNodes(neighbors);
        //for memory free
        m_nodes.append(neighbors);

        //foreach y in neighbor_nodes(x)
        for (int i = 0; i < neighbors.length(); i++) {
            AStarSearchNode* y= neighbors[i];
            //if y in closedset
            if (nodeContainedInList(closedSet, y)) {
                qDebug()<<"Node is in closedSet"<<y->nodeInfo();
                continue;
            }

            //tentative_g_score := g_score[x] + dist_between(x,y)
            float tentativeG = x->g()+x->distanceTo(*y);
            qDebug()<<"tentativeG of Node"<<tentativeG<<y->nodeInfo();
            bool tentativeIsBetter;
            //if y not in openset
            if (!nodeContainedInList(openSet, y)) {
                qDebug()<<"Node is not in openSet, add to openSet"<<y->nodeInfo();
                //add y to openset
                openSet.append(y);
                //tentative_is_better := true
                tentativeIsBetter = true;
            }
            else if (tentativeG < y->g()) {
                //elseif tentative_g_score < g_score[y]
                //tentative_is_better := true
                qDebug()<<"tentativeG < y->g()"<<tentativeG<<y->g();
                tentativeIsBetter = true;
            }
            else {
                //tentative_is_better := false
                tentativeIsBetter = false;
            }

            //tentative_is_better = true
            if (tentativeIsBetter) {
                qDebug()<<"set y from x"<<y->nodeInfo()<<x->nodeInfo();
                //came_from[y] := x
                y->setCameFrom(x);
                //g_score[y] := tentative_g_score
                y->setG(tentativeG);
                //h_score[y] := heuristic_estimate_of_distance(y, goal)
                y->setH(y->heuristicEstimateOfDistance(goal));
                //f_score[y] := g_score[y] + h_score[y]
                y->setF(y->g()+y->h());
            }
        }
    }
    m_searching = false;
    return true;
}

//function reconstruct_path(came_from,current_node)
bool AStarSearch::pathTo(const AStarSearchNode& currentNode,
                         const AStarSearchNode& targetNode,
                         QList<AStarSearchNode*>& path) {
    //if came_from[current_node] is set
    //    p = reconstruct_path(came_from,came_from[current_node])
    //       return (p + current_node)
    //else
    //    return current_node
    qDebug()<<"pathTo"<<currentNode.nodeInfo()<<targetNode.nodeInfo();
    path.clear();
    const AStarSearchNode* node = &currentNode;
    QList<AStarSearchNode*> tmpPath;
    bool found = false;
    while (node) {
        qDebug()<<"targetNode"<<targetNode.nodeInfo()
               <<"node"<<node->nodeInfo();
        if (targetNode == *node) {
            qDebug()<<"found target"<<targetNode.nodeInfo()<<node->nodeInfo();
            found = true;
            break;
        }
        tmpPath.append(const_cast<AStarSearchNode*>(node));
        node = node->cameFrom();
        if (node) {
            qDebug()<<"came from"<<node->nodeInfo();
        }
        else {
            qDebug()<<"No more came from";
        }
    }
    if (found) {
        for (int i = tmpPath.count() - 1; i >= 0; i--) {
            path.append(tmpPath[i]);
        }
        return true;
    }
    else {
        return false;
    }
}

bool AStarSearch::nodeContainedInList(const QList<AStarSearchNode*>& list,
                                      const AStarSearchNode* node) {
    for (int i = 0; i < list.count(); i++) {
        if (*list[i] == *node) {
            return true;
        }
    }
    return false;
}
